#include "modbus_tcp_server.h"

MB_TCP_MBAP TCP_server;

uint16_t TCPholdreg[64]={0,1,2,3,4,5,6,7,8,9};
uint16_t TCPinputreg[64];

uint8_t TCP_PDU_SER[64];

void TCP_mix_buffer(uint16_t PDUlen)
{
	uint8_t i,size=0;
	for(i=0;i<4;i++){
		W5500Tx_Buffer[i]=W5500Rx_Buffer[i];
		size++;
	}
	W5500Tx_Buffer[size++]=(PDUlen+1)/256;
	W5500Tx_Buffer[size++]=(PDUlen+1)%256;
	W5500Tx_Buffer[size++]=0x01;
  for(i=0;i<PDUlen;i++)
	{
		W5500Tx_Buffer[size++]=TCP_PDU_SER[i];
	}		
	Write_SOCK_Data_Buffer(0,W5500Tx_Buffer,size);
}

void function_code_03(uint16_t len_PUD)
{
	uint8_t i=0;
	uint16_t head_add=0,num_reg=0,len=0;
	head_add=W5500Rx_Buffer[7+1]*256+W5500Rx_Buffer[7+2];
	num_reg=W5500Rx_Buffer[7+3]*256+W5500Rx_Buffer[7+4];
	
	TCP_PDU_SER[0]=0x03;
	for(i=0;i<num_reg;i++)
	{
		TCP_PDU_SER[2+len]=TCPholdreg[head_add+i]/256;
		len++;
		TCP_PDU_SER[2+len]=TCPholdreg[head_add+i]%256;
		len++;
	}
	TCP_PDU_SER[1]=len;
	TCP_mix_buffer(len+2);
}


void mbtcp_pcs(void)
{
	uint8_t size;
	TCP_server.Slave_address=0x01;
	size = Read_SOCK_Data_Buffer(0, W5500Rx_Buffer);
	
	TCP_server.Protocol_id=W5500Rx_Buffer[2]*256+W5500Rx_Buffer[3];
	TCP_server.Length_messages=W5500Rx_Buffer[4]*256+W5500Rx_Buffer[5];
	if(TCP_server.Protocol_id==0x0000)
	{
		if(TCP_server.Slave_address==W5500Rx_Buffer[6])
		{
			switch (W5500Rx_Buffer[7])
			{
//			case 0x01:
//					function_code_01();
//					break;
//			case 0x02:
//					function_code_02();
//					break;
			case 0x03:
					function_code_03((TCP_server.Length_messages)-1);
					break;			
			default:
					break;
			}
		}
		else return;
	}
	else return;
	
}
